﻿/**
 * MOTOR 2 - RAPID FLASH DYNAMICS
 * Copyright (c) 2007-2008, Michael Baczynski
 * 
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 * 
 * * Redistributions of source code must retain the above copyright notice,
 *   this list of conditions and the following disclaimer.
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 * * Neither the name of the polygonal nor the names of its contributors may be
 *   used to endorse or promote products derived from this software without specific
 *   prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
package de.polygonal.motor2.collision.pairwise
{
	import de.polygonal.motor2.collision.pairwise.ClipVertex;
	import de.polygonal.motor2.collision.pairwise.FeaturePair;
	import de.polygonal.motor2.collision.shapes.ShapeSkeleton;
	import de.polygonal.motor2.dynamics.contact.Contact;
	import de.polygonal.motor2.dynamics.contact.ContactPoint;
	import de.polygonal.motor2.dynamics.contact.Manifold;
	import de.polygonal.motor2.dynamics.contact.generator.PolyContact;
	import de.polygonal.motor2.math.ConvexBSPNode;
	import de.polygonal.motor2.math.V2;	

	public class CollidePoly implements Collider
	{
		private static const _clipVertexIncEdge0:ClipVertex = new ClipVertex();
		private static const _clipVertexIncEdge1:ClipVertex = new ClipVertex();
		
		private static const _clipVertexCp0:ClipVertex = new ClipVertex();
		private static const _clipVertexCp1:ClipVertex = new ClipVertex();
		
		/**
		 *   notes:
		 * - sat time complexity: O(2 * n0 * n1)
		 * - requirements: vertex list in clockwise order,
		 *   shape vertices and normals in world space.
		 * - the perp operator is defined as perp(x, y) = (y, -x)
		 *   which is the outward pointing normal vector (to the left
		 *   of the original vector.
		 * - separation distance is computed by applying weightings
		 *   to prefer one axis over another. (ERIN - improved coherence).
		 * - sat query computes minimum separation distance (> 0) or 0
		 *   if no intersection was found.
		 * - collision normal: reference body -> incident body
		 */
		public function collide(manifold:Manifold, s1:ShapeSkeleton, s2:ShapeSkeleton, contact:Contact):void
		{
			var ncollx:Number, frontNormalx:Number;
			var ncolly:Number, frontNormaly:Number;
			var p:V2, d:V2, e:V2, min:V2, N:V2;
			var sep:Number, depth:Number, flip:Boolean, last:int;
			
			var node:ConvexBSPNode;
			
			var sRef:ShapeSkeleton, sInc:ShapeSkeleton;
			
			var refFaceVertex:V2;
			var incFaceVertex:V2;
			var incFaceNormal:V2;
			
			var c:PolyContact = PolyContact(contact);
			
			//without hinting, used temorary for testing
			/*sep = -2147483648;
			p = s1.worldVertexChain;
			d = s1.worldNormalChain.prev;
			last = p.prev.I;
			
			while (true)
			{
				node = s2.BSPNode;
				while (node.R)
				{
					e = node.N;
					//dot(e, d) eq. dot(perp(n), d)
					node = (-e.y * d.x + e.x * d.y <= 0) ? node.R : node.L;
				}
				min = node.V;
				
				var tt:V2 = min;
				v = min = s2.worldVertexChain;
				dmin = v.x * d.x + v.y * d.y;
				
				for (j = 1; j < s2.vertexCount; j++)
				{
					v = v.next;
					proj = v.x * d.x + v.y * d.y;
					if (proj < dmin)
					{
						dmin = proj;
						min = v;
					}
				}
				if (tt.I != min.I) SOSTrace.tr(">" + (tt.I + "," + min.I));
					
				depth = d.x * (min.x - p.x) + d.y * (min.y - p.y);
				if (depth > 0)
				{
					manifold.pointCount = 0;
					return;
				}
				if (depth * .95 + .01 > sep)
				{
					sep = depth;
					N = d;
					
					refFaceVertex = p.prev;
					
					incFaceNormal = node.N;
					incFaceVertex = node.V;
				}
				
				if (p.I == last) break;
				p = p.next;
				d = d.next;
			}
			
			p = s2.worldVertexChain;
			d = s2.worldNormalChain.prev;
			last = p.prev.I;
			
			while (true)
			{
				node = s1.BSPNode;
				while (node.R)
				{
					e = node.N;
					node = (-e.y * d.x + e.x * d.y <= 0) ? node.R : node.L;
				}
				min = node.V;
				depth = d.x * (min.x - p.x) + d.y * (min.y - p.y);
				if (depth > 0)
				{
					manifold.pointCount = 0;
					return;
				}
				if (depth * .95 + .01 > sep)
				{
					sep = depth;
					N = d;
					
					refFaceVertex = p.prev;
					flip = true;
					
					incFaceNormal = node.N;
					incFaceVertex = node.V;
				}
				
				if (p.I == last) break;
				p = p.next;
				d = d.next;
			}
			*/
			
			
			var dx:Number, dy:Number, v:V2, j:int, proj:Number, dmin:Number;
			
			
			
			//{separating-axis test
			if (c.firstShapeExit)
			{
				sep = -2147483648;
				flip = false;
				
				p = c.p;
				d = c.n;
				//WTF?
				p = s1.worldVertexChain;
				d = s1.worldNormalChain.prev;
				last = p.prev.I;
				
				dx = d.x;
				dy = d.y;
				
				while (true)
				{
					node = s2.BSPNode;
					while (node.R)
					{
						e = node.N;
						//dot(e, d) eq. dot(perp(n), d)
						node = (-e.y * d.x + e.x * d.y <= 0) ? node.R : node.L;
					}
					min = node.V;
				
//				var min0:Number;
//			var min1:Number;
//			var min2:Number;
//					min1 = e.x * dx + e.y * dy;
//					while (true)
//					{
//						min0 = e.prev.x * dx + e.prev.y * dy;
//						min2 = e.next.x * dx + e.next.y * dy;
//						if (min0 < min1)
//						{
//							e = e.prev;
//							min1 = min0;
//						}
//						else
//						if (min2 < min1)
//						{
//							e = e.next;
//							min1 = min2;
//						}
//						else
//							break;
//					}
					
					depth = d.x * (min.x - p.x) + d.y * (min.y - p.y);
					
					if (depth > 0)
					{
						//store hint for next query
						c.p = p;
						c.n = d;
						manifold.pointCount = 0;
						return;
					}
					
					if (depth * .95 + .01 > sep)
					{
						sep = depth;
						N = d;
						
						refFaceVertex = p.prev;
						
						incFaceNormal = node.N;
						incFaceVertex = node.V;
					}
					
					if (p.I == last) break;
					p = p.next;
					d = d.next;
				}
				
				p = s2.worldVertexChain;
				d = s2.worldNormalChain.prev;
				last = p.prev.I;
				
				while (true)
				{
					node = s1.BSPNode;
					while (node.R)
					{
						e = node.N;
						node = (-e.y * d.x + e.x * d.y <= 0) ? node.R : node.L;
					}
					min = node.V;
					
					depth = d.x * (min.x - p.x) + d.y * (min.y - p.y);
					
					if (depth > 0)
					{
						c.p = p;
						c.n = d;
						c.firstShapeExit = false;
						manifold.pointCount = 0;
						return;
					}
					
					if (depth * .95 + .01 > sep)
					{
						sep = depth;
						N = d;
						refFaceVertex = p.prev;
						flip = true;
						
						incFaceNormal = node.N;
						incFaceVertex = node.V;
					}
					
					if (p.I == last) break;
					p = p.next;
					d = d.next;
				}
			}
			else
			{
				sep = -2147483648;
				flip = true;
				
				p = c.p;
				d = c.n;
				
				p = s2.worldVertexChain;
				d = s2.worldNormalChain.prev;
				last = p.prev.I;
				
				while (true)
				{
					node = s1.BSPNode;
					while (node.R)
					{
						e = node.N;
						node = (-e.y * d.x + e.x * d.y <= 0) ? node.R : node.L;
					}
					min = node.V;
					
					depth = d.x * (min.x - p.x) + d.y * (min.y - p.y);
					if (depth > 0)
					{
						c.p = p;
						c.n = d;
						manifold.pointCount = 0;
						return;
					}
					
					if (depth * .95 + .01 > sep)
					{
						sep = depth;
						N = d;
						refFaceVertex = p.prev;
						
						incFaceNormal = node.N;
						incFaceVertex = node.V;
					}
					
					if (p.I == last) break;
					p = p.next;
					d = d.next;
				}
				
				p = s1.worldVertexChain;
				d = s1.worldNormalChain.prev;
				last = p.prev.I;
				
				while (true)
				{
					node = s2.BSPNode;
					while (node.R)
					{
						e = node.N;
						node = (-e.y * d.x + e.x * d.y <= 0) ? node.R : node.L;
					}
					min = node.V;
					
					depth = d.x * (min.x - p.x) + d.y * (min.y - p.y);
					
					if (depth > 0)
					{
						c.p = p;
						c.n = d;
						c.firstShapeExit = true;
						manifold.pointCount = 0;
						return;
					}
					
					if (depth * .95 + .01 > sep)
					{
						sep = depth;
						N = d;
						
						refFaceVertex = p.prev;
						flip = false;
						
						incFaceNormal = node.N;
						incFaceVertex = node.V;
					}
					
					if (p.I == last) break;
					p = p.next;
					d = d.next;
				}
			
			}
			
			if (flip)
			{
				ncollx =-N.x;
				ncolly =-N.y;
			}
			else
			{
				ncollx = N.x;
				ncolly = N.y;
			}
			//}
			
			//{determine reference/incident shape & frontNormal
			var flipFeatures:Boolean; //same as flip mr. stupido
			if (flip)
			{
				sRef = s2;
				sInc = s1;
				frontNormalx =-ncollx;
				frontNormaly =-ncolly;
				flipFeatures = true;
			}
			else
			{
				sRef = s1;
				sInc = s2;
				frontNormalx = ncollx;
				frontNormaly = ncolly;
				flipFeatures = false;
			}
			//}
			
			//{compute incident face
			var t0:Number, t1:Number;
			
			//TODO optimize!
			
			var incFaceVertexBruteForce:V2 = null;
			
			p = sInc.worldVertexChain;
			N = sInc.worldNormalChain;
			var tmin:Number = Number.MAX_VALUE;
			while (true)
			{
				var t:Number = frontNormalx * N.x + frontNormaly * N.y;
				if (t < 0)
				{
					if (t < tmin)
					{
						tmin = t;
						incFaceVertexBruteForce = p;
					}
				}
				if (N.last) break;
				N = N.next;
				p = p.next;
			}
			var check0:int = incFaceVertexBruteForce.I;
			
			var incVert:V2 = incFaceVertex;
			
			//hill-climbing, check min - 1 and min + 1
			/*t0 = incFaceNormal.x      * frontNormalx + incFaceNormal.y      * frontNormaly;
			t1 = incFaceNormal.prev.x * frontNormalx + incFaceNormal.prev.y * frontNormaly;
			if (t1 < t0)
				incFaceVertex = incFaceVertex.prev;
			else
			{
				t1 = incFaceNormal.next.x * frontNormalx + incFaceNormal.next.y * frontNormaly;
				if (t1 < t0)
					incFaceVertex = incFaceVertex.next;
			}
			
			var check2:int = incFaceVertex.I
			
			if (check0 != check2)
			{
				Main.tr(check0, check2);
				//Main.main.stop();
			}*/
			
			incFaceVertex = incFaceVertexBruteForce;
			
			//}
			
			//{compute clipping planes (reference face side planes)
			var front:Number = (sRef.x * frontNormalx + sRef.y * frontNormaly) + sRef.faceExt[refFaceVertex.I];
			
			//'positive' side normal
			var sideNormalx:Number = -frontNormaly;
			var sideNormaly:Number =  frontNormalx;
			
			//contact id & clipping 
			var negEdge:int, negSide:Number;
			var posEdge:int, posSide:Number;
			
			var side:Number;
			
			if (sRef.regularShape)
				side = sRef.x * sideNormalx + sRef.y * sideNormaly;
			else
			{
				var offset:V2 = sRef.offsets[refFaceVertex.I];
				var xOffset:Number = sRef.x + sRef.r11 * offset.x + sRef.r12 * offset.y;
				var yOffset:Number = sRef.y + sRef.r21 * offset.x + sRef.r22 * offset.y;
				
				side = xOffset * sideNormalx + yOffset * sideNormaly;
			}
			
			var edgeExt:Number = sRef.edgeExt[refFaceVertex.I];
			
			negSide =-side + edgeExt; negEdge = refFaceVertex.prev.I;
			posSide = side + edgeExt; posEdge = refFaceVertex.next.I;
			//}
			
			//{setup incident face
			
			var incEdgeVertexId0:int = incFaceVertex.I
			_clipVertexIncEdge0.x    = incFaceVertex.x;
			_clipVertexIncEdge0.y    = incFaceVertex.y;
			
			var incEdgeVertexId1:int = incFaceVertex.next.I;
			_clipVertexIncEdge1.x    = incFaceVertex.next.x;
			_clipVertexIncEdge1.y    = incFaceVertex.next.y;
			
			_clipVertexIncEdge0.fp.inEdge1  = incFaceVertex.prev.I;
			_clipVertexIncEdge0.fp.outEdge1 = incFaceVertex.I;
			_clipVertexIncEdge1.fp.inEdge2  = incFaceVertex.I;
			_clipVertexIncEdge1.fp.outEdge2 = incFaceVertex.next.next.I;
			//}
			
			//{compute contact points
			var incEdgeCv0:ClipVertex = _clipVertexIncEdge0;
			var incEdgeCv1:ClipVertex = _clipVertexIncEdge1;
			
			var cvOut0:ClipVertex = _clipVertexIncEdge0;
			var cvOut1:ClipVertex = _clipVertexIncEdge1;
			
			var dist0:Number, dist1:Number, interp:Number;
			
			//<--| clip to negative side
			dist0 = incEdgeCv0.x * -sideNormalx + incEdgeCv0.y * -sideNormaly - negSide;
			dist1 = incEdgeCv1.x * -sideNormalx + incEdgeCv1.y * -sideNormaly - negSide;
			
			//on different sides of the clipping plane
			if (dist0 * dist1 < 0)
			{
				//find intersection edge-plane
				interp = dist0 / (dist0 - dist1);
				
				//behind the plane
				cvOut0 = dist0 < 0 ? incEdgeCv0 : incEdgeCv1;
				
				//on the plane
				cvOut1.x = incEdgeCv0.x + interp * (incEdgeCv1.x - incEdgeCv0.x);
				cvOut1.y = incEdgeCv0.y + interp * (incEdgeCv1.y - incEdgeCv0.y);
				
				if (dist0 > 0)
				{
					cvOut1.fp = incEdgeCv0.fp;
					cvOut1.fp.inEdge1 = negEdge;
					cvOut1.fp.inEdge2 = FeaturePair.NO_EDGE;
				}
				else
				{
					cvOut1.fp = incEdgeCv1.fp;
					cvOut1.fp.outEdge1 = negEdge;
					cvOut1.fp.outEdge2 = FeaturePair.NO_EDGE;
				}
			}
			else
			{
				//both points in front of the plane
				if (dist0 > 0 && dist1 > 0)
				{
					manifold.pointCount = 0;
					return;
				}
				
				//both points behind the plane
				if (dist0 < dist1)
				{
					cvOut0 = incEdgeCv0;
					cvOut1 = incEdgeCv1;
				}
				else
				{
					cvOut1 = incEdgeCv0;
					cvOut0 = incEdgeCv1;
				}
			}
			
			//clip to positive side |-->
			dist0 = cvOut0.x * sideNormalx + cvOut0.y * sideNormaly - posSide;
			dist1 = cvOut1.x * sideNormalx + cvOut1.y * sideNormaly - posSide;
			
			if (dist0 * dist1 < 0)
			{
				interp = dist0 / (dist0 - dist1);
				
				cvOut0.x = cvOut0.x + interp * (cvOut1.x - cvOut0.x);
				cvOut0.y = cvOut0.y + interp * (cvOut1.y - cvOut0.y);
				
				if (dist0 > 0)
				{
					cvOut0.fp = cvOut0.fp;
					cvOut0.fp.inEdge1 = posEdge;
					cvOut0.fp.inEdge2 = FeaturePair.NO_EDGE;
				}
				else
				{
					cvOut0.fp = cvOut1.fp;
					cvOut0.fp.outEdge1 = posEdge;
					cvOut0.fp.outEdge2 = FeaturePair.NO_EDGE;
				}
			}
			else
			{
				if (dist0 > 0)
				{
					manifold.pointCount = 0;
					return;
				}
			}
			//}
			
			//{output contact manifold
			
			//cvOut0 and cvOut1 are potential clipping points.
			//ERIN: due to roundoff, it is possible that clipping removes all points.
			var cp:ContactPoint;
			sep = frontNormalx * cvOut0.x + frontNormaly * cvOut0.y - front;
			if (sep <= 0)
			{
				manifold.pointCount = 1;
				manifold.nx = ncollx;
				manifold.ny = ncolly;
				
				cp = manifold.c0;
				cp.sep = sep;
				cp.x   = cvOut0.x;
				cp.y   = cvOut0.y;
				
				//TODO optimize inline feature copy and flip
				cp.feature.paste(cvOut0.fp);
				if (flipFeatures) cp.feature.flip();
				
				sep = frontNormalx * cvOut1.x + frontNormaly * cvOut1.y - front;
				if (sep <= 0)
				{
					manifold.pointCount = 2;
					
					cp = manifold.c1;
					cp.sep = sep;
					cp.x   = cvOut1.x;
					cp.y   = cvOut1.y;
					
					cp.id.incEdge = incFaceVertex.I;
					cp.id.refFace = refFaceVertex.I;
					cp.id.flip    = flipFeatures ? 1 : 0;
					cp.id.incVert = incFaceVertex.next.I;
					
					cp.feature.paste(cvOut1.fp);
					if (flipFeatures) cp.feature.flip();
				}
			}
			else
			{
				sep = frontNormalx * cvOut1.x + frontNormaly * cvOut1.y - front;
				if (sep <= 0)
				{
					manifold.pointCount = 1;
					manifold.nx = ncollx;
					manifold.ny = ncolly;
					
					cp = manifold.c0;
					cp.sep = sep;
					cp.x   = cvOut1.x;
					cp.y   = cvOut1.y;
					
					cp.id.incEdge = incFaceVertex.I;
					cp.id.refFace = refFaceVertex.I;
					cp.id.flip    = flipFeatures ? 1 : 0;
					cp.id.incVert = incFaceVertex.next.I;
					
					cp.feature.paste(cvOut1.fp);
					if (flipFeatures) cp.feature.flip();
					manifold.pointCount = 1;
				}
			}
			//}
		}
	}
}
